#include "powercontroller.h"
#include "dji.h"
#include "motor_def.h"
#include <math.h>
#include "controller.h"


#define LOG_TAG              "powercontrol"
#define LOG_LVL              LOG_LVL_DBG
#include <ulog.h>

#define TOQUE_COEFFICIENT 1.99688994e-6f

static PowerControl_t powercontrol;

void powercontrol_init()
{
    for (size_t i = 0; i < 4; i++)
    {
        powercontrol.K1[i]=1.23e-07;
        powercontrol.K2[i]=1.453e-07;
        powercontrol.constant[i] = 4.081f; //电机输出轴实际转矩，单位N·m
        powercontrol.power_control_out[i] = 0;
        powercontrol.initial_give_power[i] = 0;
    }

    PID_Init_Config_s config = {.MaxOut = 20,
        .IntegralLimit = 20,
        .DeadBand = 5,
        .Kp = 0.1,
        .Ki = 0,
        .Kd = 0,
        .Improve = 0x01}; // enable integratiaon limit
    PIDInit(&powercontrol.buffer_energy_pid, &config);
}

/**
 * @brief 设置底盘的最大功率限制
 *
 * @param power_limit 限制的功率值
 */
void SetPowerLimit(float power_limit)
{
    powercontrol.chassis_max_power = power_limit-5; //保留5w余量
}



rt_inline int16_t currentToInteger(float I_min,float I_max,int16_t V_min,int16_t V_max,float current) {
    int16_t V = (int)((current - I_min) / (I_max - I_min) * (V_max - V_min) + V_min);
    if (V >V_max)
    {
        V=V_max;
    }
    if (V<V_min)
    {
        V=V_min;
    }
    return V;
}

void PowerControlDji(DJIMotor_t *motor)
{
    // 检查电机的功率控制状态是否开启
    if (motor->motor_settings.PowerControlState != PowerControlState_ON)
    {
        return; // 如果功率控制未开启，直接返回
    }
    switch (motor->motor_settings.control_algorithm) 
    {
        case CONTROL_PID:
        {
            float pid_measure, pid_ref;             // 电机PID测量值和设定值
            pid_ref = motor->motor_controller.pid_ref; // 保存设定值,防止motor_controller->pid_ref在计算过程中被修改
            if (motor->motor_settings.motor_reverse_flag == MOTOR_DIRECTION_REVERSE)
                pid_ref *= -1;                         // 设置反转

            // 计算速度环,(外层闭环为速度或位置)且(启用速度环)时会计算速度环
            if ((motor->motor_settings.close_loop_type & SPEED_LOOP) && (motor->motor_settings.outer_loop_type & (ANGLE_LOOP | SPEED_LOOP)))
            {
                if (motor->motor_settings.feedforward_flag & SPEED_FEEDFORWARD)
                    pid_ref += *motor->motor_controller.speed_feedforward_ptr;

                if (motor->motor_settings.speed_feedback_source == OTHER_FEED)
                    pid_measure = *motor->motor_controller.other_speed_feedback_ptr;
                else // MOTOR_FEED
                    pid_measure = motor->measure.speed_rpm;
                // 更新pid_ref进入下一个环
                pid_ref = PIDCalculate(&motor->motor_controller.speed_PID, pid_measure, pid_ref);
            }

            // 计算电流环,目前只要启用了电流环就计算,不管外层闭环是什么,并且电流只有电机自身传感器的反馈
            if (motor->motor_settings.feedforward_flag & CURRENT_FEEDFORWARD)
                pid_ref += *motor->motor_controller.current_feedforward_ptr;
            if (motor->motor_settings.close_loop_type & CURRENT_LOOP)
            {
                pid_ref = PIDCalculate(&motor->motor_controller.current_PID, motor->measure.real_current, pid_ref);
            }          

            if (motor->motor_settings.feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE) pid_ref *= -1;

            powercontrol.power_control_out[motor->message_num] = pid_ref;

            float A = powercontrol.K1[motor->message_num] * pid_ref * pid_ref;
            float B = powercontrol.K2[motor->message_num] * motor->measure.speed_rpm * motor->measure.speed_rpm;
            float C = TOQUE_COEFFICIENT * motor->measure.speed_rpm * pid_ref;
            powercontrol.initial_give_power[motor->message_num] = A + B + C + powercontrol.constant[motor->message_num];

            powercontrol.real_power[motor->message_num] = TOQUE_COEFFICIENT * motor->measure.real_current * motor->measure.speed_rpm
                                                                      + powercontrol.K1[motor->message_num] * motor->measure.speed_rpm * motor->measure.speed_rpm 
                                                                      + powercontrol.K2[motor->message_num] * motor->measure.real_current * motor->measure.real_current
                                                                      + powercontrol.constant[motor->message_num];
            break;
        }
        case CONTROL_LQR:
        {
            float state0, state1;
            if(motor->motor_settings.motor_reverse_flag == MOTOR_DIRECTION_REVERSE)
            {
                //motor->motor_controller.lqr.feedbackreverseflag = 1;
                motor->motor_controller.lqr_ref *=-1;  
            }

            // 计算位置环,只有启用位置环且外层闭环为位置时会计算速度环输出
            if ((motor->motor_settings.close_loop_type & ANGLE_LOOP) && motor->motor_settings.outer_loop_type == ANGLE_LOOP)
            {   
                if (motor->motor_settings.angle_feedback_source == OTHER_FEED) {state0 = *motor->motor_controller.other_angle_feedback_ptr;}
                else {state0 = motor->measure.total_angle;} // MOTOR_FEED,对total angle闭环,防止在边界处出现突跃
            }

            if ((motor->motor_settings.close_loop_type & SPEED_LOOP) && (motor->motor_settings.outer_loop_type & (ANGLE_LOOP | SPEED_LOOP)))
            {   
                if (motor->motor_settings.speed_feedback_source == OTHER_FEED)  {state1 = *motor->motor_controller.other_speed_feedback_ptr;}
                else  {state1 = motor->measure.speed_aps;}
            }

            float torque = LQRCalculate(&motor->motor_controller.lqr, state0, state1, motor->motor_controller.lqr_ref);

            if (motor->motor_settings.feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE) torque *= -1;

            switch (motor->motor_type) 
            {                                          
                case M3508:
                    {
                        float lqr_out;
                        lqr_out = currentToInteger(-20.0f, 20.0f, -16384, 16384, (torque / (0.3f))); // 力矩常数（Nm/A）
                        powercontrol.power_control_out[motor->message_num] = lqr_out;

                        float A = powercontrol.K1[motor->message_num] * lqr_out * lqr_out;
                        float B = powercontrol.K2[motor->message_num] * motor->measure.speed_rpm * motor->measure.speed_rpm;
                        float C = TOQUE_COEFFICIENT * motor->measure.speed_rpm * lqr_out;
                        powercontrol.initial_give_power[motor->message_num] = A + B + C + powercontrol.constant[motor->message_num];

                        powercontrol.real_power[motor->message_num] = TOQUE_COEFFICIENT * motor->measure.real_current * motor->measure.speed_rpm
                                                                      + powercontrol.K1[motor->message_num] * motor->measure.speed_rpm * motor->measure.speed_rpm 
                                                                      + powercontrol.K2[motor->message_num] * motor->measure.real_current * motor->measure.real_current
                                                                      + powercontrol.constant[motor->message_num];
                        break;                                
                    }
                default :
                    break;
            }
            break;
        }
        case CONTROL_OTHER:
            // 待加入
            break;
        default:
            break;
    }
}



// 计算总功率并进行功率分配
void PowerControlDjiFinalize(rt_slist_t *motor_list)
{
    powercontrol.initial_total_power = 0;
    powercontrol.real_total_power = 0;
    rt_slist_t *node = RT_NULL;
    DJIMotor_t *motor = RT_NULL;

    #if USING_REFREE_SYSTEM ==1 
        read_referee_powerinfo(&powercontrol.chassis_max_power,&powercontrol.buffer_energy);
        powercontrol.chassis_max_power -=5;
        // 使用PID控制器调整chassis_max_power，使buffer_energy维持在30左右
        float pid_output = PIDCalculate(&powercontrol.buffer_energy_pid, powercontrol.buffer_energy, 40.0f);
        powercontrol.chassis_max_power -= pid_output;
    #else
        powercontrol.chassis_max_power=80;
        powercontrol.buffer_energy=0;
    #endif
    
    // 遍历所有电机实例,进行功率计算
    rt_slist_for_each(node, motor_list)
    {
        motor = rt_slist_entry(node, DJIMotor_t, list);
        if (motor->motor_settings.PowerControlState == PowerControlState_ON)
        {
            if (powercontrol.initial_give_power[motor->message_num] < 0) { continue; }
            powercontrol.initial_total_power += powercontrol.initial_give_power[motor->message_num];
            powercontrol.real_total_power    += powercontrol.real_power[motor->message_num];
        }
    }

    if (powercontrol.initial_total_power > powercontrol.chassis_max_power)
    {
        float ratio = powercontrol.chassis_max_power / powercontrol.initial_total_power; // 根据允许的最大功率进行放缩
        rt_slist_for_each(node, motor_list)
        {
            motor = rt_slist_entry(node, DJIMotor_t, list);
            if (motor->motor_settings.PowerControlState == PowerControlState_ON)
            {
                //(分配策略)
                powercontrol.scaled_give_power[motor->message_num] = powercontrol.initial_give_power[motor->message_num] * ratio; // 根据允许的最大功率进行放缩
                //（均分策略）
                //powercontrol.scaled_give_power[motor->message_num] = powercontrol.chassis_max_power/4;
                if (powercontrol.scaled_give_power[motor->message_num]< 0) { continue;}   // 功率小于零，表明依靠发电减速，因此不算入功率统计

                float a = powercontrol.K1[motor->message_num];
                float b = TOQUE_COEFFICIENT * motor->measure.speed_rpm;
                float c = powercontrol.K1[motor->message_num] * motor->measure.speed_rpm * motor->measure.speed_rpm - powercontrol.scaled_give_power[motor->message_num] + powercontrol.constant[motor->message_num];
                powercontrol.inside[motor->message_num] = b * b -4 * a * c;
                if (powercontrol.inside[motor->message_num]<0) { continue;}

                if (powercontrol.power_control_out[motor->message_num] > 0)
                {
                    float  temp = (-b + sqrt(powercontrol.inside[motor->message_num])) / (2 * a);
                    powercontrol.power_control_out[motor->message_num] = temp;
                    if (powercontrol.power_control_out[motor->message_num] > 15000)
                    {
                        powercontrol.power_control_out[motor->message_num] = 15000;
                    }
                }
                if (powercontrol.power_control_out[motor->message_num] < 0)
                {
                    float  temp = (-b - sqrt(powercontrol.inside[motor->message_num])) / (2 * a);
                    powercontrol.power_control_out[motor->message_num] = temp;
                    if (powercontrol.power_control_out[motor->message_num] < -15000)
                    { 
                        powercontrol.power_control_out[motor->message_num] = -15000;
                    }
                }
            }
        }
    }
}

// 获取电机的功率控制输出
int16_t GetPowerControlOutput(uint8_t motor_num)
{

    return (int16_t)powercontrol.power_control_out[motor_num];
}
